#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int32.hpp>
#include "yolo_detect_message/msg/traker_boxs.hpp"
#include "yolo_detect_message/msg/traker_box.hpp"
#include "yolo_detect_message/msg/detect_boxs.hpp"
#include  "yolo_detect_message/msg/detect_box.hpp"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <thread>
#include <algorithm>

#include <mutex>
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"

// 自主攻击算法能够控制无人机进行自主攻击，通作过订阅目标检测的消息，并根据预设的算法逻辑来控制无人机的飞行轨迹和攻击动。

class Attack : public rclcpp::Node
{
public:
    Attack();
    ~Attack(){}


    // 算法状态的回调函数
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);

    void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);
    // 目标检测识别的回调函数
    void yolo_callback(const yolo_detect_message::msg::DetectBoxs::SharedPtr msg);

    // 无人机打击的目标对象id的回调函数
    void attack_id_callback(const std_msgs::msg::Int32::SharedPtr msg);

    // 重置无人机的速度信息，默认为0
    void reset_velocity();

    // 设置无人机的速度信息
    void set_velocity(float x, float y);
    float clamp(float value,float max_value);
    // 无人机的定时器回调函数
    void timer_callback();

    bool compareBydistance(const  yolo_detect_message::msg::DetectBox & a, const yolo_detect_message::msg::DetectBox& b) {
        
        float half_width = image_width / 2;
        float half_height = image_height / 2;
        float a_target_x = a.xmin + a.width / 2.0;
        float a_target_y = a.ymin + a.heigh / 2.0;

        float b_target_x = b.xmin + b.width / 2.0;
        float b_target_y = b.ymin + b.heigh / 2.0;

        float distance_a = sqrt(pow((half_width - a_target_x) , 2) + pow((half_height - a_target_y), 2));
        float distance_b = sqrt(pow((half_width - b_target_x) , 2) + pow((half_height - b_target_y), 2));
        return distance_a < distance_b;
    }
    void echo();
private:

    // ------------------------无人机相关的话题节点信息---------------------------------------
    // 订阅m目标检测的话题消息
    std::string yolo_topic_name;
    rclcpp::Subscription<yolo_detect_message::msg::DetectBoxs>::SharedPtr yolo_sub;


    // 订阅算法的启动节点的话题消息
    std::string start_topic_name;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;
    // 当前算分的服务状态，默认为false
    std::mutex service_status_mutex;
    // bool service_status;

  
    // 发布无人机可以进行打击的行为
    std::string attack_topic_name;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr attack_pub;

    // 发布无人机速度信息的节点
    rclcpp::TimerBase::SharedPtr timer_;

    // 发布无人机移动的接口信息
    rclcpp::Publisher<yolo_detect_message::msg::MoveInterface>::SharedPtr move_pub_;
    std::string move_topic_name;

    // 订阅用户的打击的目标对象的id信息
    std::string attack_id_topic_name;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr attack_id_sub_;
    // -------------------------------------------------------------------------------------


    // ---------------------------无人机进行目标打击时的全局状态或者类别信息----------------------
    // 被打击对象的id信息，默认为0
    // 单目标对象为无人机的类别信息，多目标对象为目标的编号表示信息
    std::mutex attack_id_mutex;
    int attack_id = 0;

    // 无人机当前的速度信息，默认为0
    std::mutex current_velocity_mutex;
    geometry_msgs::msg::Twist current_velocity;

    // 无人机进行目标打击时的速度信息，默认为0.5m/s
    float default_x_velocity = 0.5;
    float default_y_velocity = 0.5;

    // 表示当前的像素点的误差范围，默认为100px
    float plane_error_threshold = 100;

    // 无人机获取图像的宽和高信息，默认为1920x1080
    int image_width = 1920;
    int image_height = 1080;

    // 目标对象在图像中的坐标信息，默认为0
    int target_location_width = 0;
    int target_location_height = 0;


    // 目标是否在当前图像的中心位置附近，默认为false
    std::mutex is_target_in_center_mutex;
    bool is_target_in_center = false;


    // 无人机是否第一次找到目标对象，默认为true
    bool first_find_target = true;
    int wait_time = 10;
    // ---------------------------------------------------------------------------------

    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;
    std::atomic<bool> service_status{false};

    int vehicle_id_;
    std::string namespace_;


};